/* =================================================================================
File name:       PI.H 
===================================================================================*/


#ifndef __PI_H__
#define __PI_H__

#include "../mlib/mlib.h"

typedef struct
{
	F32  Ref;   			/**<  Input: reference set-point */
	F32  Fbk;   			/**<  Input: feedback */
	F32  Out;   			/**<  Output: controller output */
	F32  Kp;				/**<  Parameter: proportional loop gain */
	F32  Ki;			    /**<  Parameter: integral gain */
	F32  OutMax;			/**<  Parameter: upper saturation limit */
	F32  OutMin;			/**<  Parameter: lower saturation limit */
	F32  Err;				/**<  Data: Error */
	F32  Up;				/**<  Data: proportional term */
	F32  Ui;				/**<  Data: integral term */
	F32  OutPreSat;			/**<  Data: pre-saturated controller output */
} PI_CONTROLLER;

/*------------------------------------------------------------------------------
 	PI_Macro Definition
------------------------------------------------------------------------------*/

STATIC_INLINE void PI_Calc( PI_CONTROLLER *p )
{
	/* Compute the error */
	p->Err = p->Ref - p->Fbk;
	/* Compute the proportional output */
	p->Up = p->Kp * p->Err;
	/* Compute the integral output */
	p->Ui = p->Ui + p->Ki * p->Up;
	/* Saturate the Ui */
	p->Ui = middle_f32(p->OutMin,p->Ui,p->OutMax);
	/* Compute the pre-saturated output */
	p->OutPreSat = p->Up + p->Ui;
	/* Saturate the output */
	p->Out = middle_f32(p->OutMin,p->OutPreSat,p->OutMax);
}




#endif /*  __PI_H__ */

